#include <stdio.h>
#include <math.h>
#include <signal.h>
#include <sys/mman.h>
#include <rtdm/rtdm.h>
#include <native/task.h>
#include <native/sem.h>
#include <native/mutex.h>
#include <native/timer.h>
#include <rtdk.h>
#include <pthread.h>

#include "ecrt.h"

RT_TASK my_task;

#define SYNC_TO_MASTER 1

static int run = 1;

#define TEST_MASK(DATA, VALUE) ((DATA & VALUE##_MASK) == VALUE)

#define CW_SHUTDOWN 0x6
#define CW_SWITCH_ON 0x7
#define CW_DISABLE_VOLTAGE 0x0
#define CW_QUICK_STOP 0x2
#define CW_DISABLE_OPERATION 0x7
#define CW_ENABLE_OPERATION 0xf
#define CW_FAULT_RESET_LOW 0x0
#define CW_FAULT_RESET_HIGH 0x10

#define SW_SWITCH_ON_DISABLED 0x40
#define SW_SWITCH_ON_DISABLED_MASK 0x4f
#define SW_READY_TO_SWITCH_ON 0x21
#define SW_READY_TO_SWITCH_ON_MASK 0x6f
#define SW_SWITCHED_ON 0x23
#define SW_SWITCHED_ON_MASK 0x6f
#define SW_OPERATION_ENABLED 0x27
#define SW_OPERATION_ENABLED_MASK 0x6f
#define SW_QUICK_STOP_ACTIVE 0x7
#define SW_QUICK_STOP_ACTIVE_MASK 0x6f
#define SW_FAULT_REACTIVE 0xf
#define SW_FAULT_REACTIVE_MASK 0x4f
#define SW_FAULT 0x8
#define SW_FAULT_MASK 0x4f

// AssignActive Values (ESC 0x0980)
#define ACTIVATE_CYCLIC_OPERATION (0x1 << 8)
#define ACTIVATE_SYNC0 (0x1 << 8 + 1)
#define ACTIVATE_SYNC1 (0x1 << 8 + 2)

static int all_ready = 0;
static int ready_count = 0;

static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};

static uint8_t *domain1_pd = NULL;

static ec_slave_config_t *slaves[6] = {NULL}; // Array to store slave config pointers
static ec_slave_config_t *slave0 = NULL;


#define PERIOD_NS 1000000

// #define PANASONIC_A6BE 0x0000066f, 0x60380004
// #define PANASONIC_A6BE 0xab, 0x1030
#define PANASONIC_A6BE 0x748, 0xfffe

static uint32_t position_slaves[6] = {0}; // Array to store positions for each slave

static uint32_t position_slave0 = 0;

#define INDEX_VENDOR_ID 0x1018, 0x1
#define INDEX_PRODUCT_CODE 0x1018, 0x2

/* Master 0, Slaves "MADLN05BE"
 * Vendor ID:       0x0000066f
 * Product code:    0x60380004
 * Revision number: 0x00010000
 */

// ec_pdo_entry_info_t slave_pdo_entries[] = {
//     {0x6040, 0x00, 16}, /* Controlword */
// //    {0x6060, 0x00, 8},  /* Modes of operation */
//     {0x607a, 0x00, 32}, /* Target position */
// 	{0x60b1, 0x00, 32},
// 	{0x60b2, 0x00, 16},
// //    {0x60b8, 0x00, 16}, /* Touch probe function */
//     {0x603f, 0x00, 16}, /* Error code */
//     {0x6041, 0x00, 16}, /* Statusword */
// //    {0x6061, 0x00, 8},  /* Modes of operation display */
//     {0x6064, 0x00, 32}, /* Position actual value */
// //    {0x60b9, 0x00, 16}, /* Touch probe status */
// //    {0x60ba, 0x00, 32}, /* Touch probe pos1 pos value */
//     {0x60f4, 0x00, 32}, /* Following error actual value */
// 	{0x606C, 0x00, 32},
// 	{0x6077, 0x00, 16},
// 	{0x2202, 0x00, 16},
// 	{0x2200, 0, 16}
// //    {0x60fd, 0x00, 32}, /* Digital inputs */
// };
ec_pdo_entry_info_t slave_pdo_entries[] = {
	    {0x6040, 0x00, 16}, /* Controlword */
	    {0x6060, 0x00, 8}, /* Modes of operation */
	    {0x607a, 0x00, 32}, /* Target position */
	    {0x60b8, 0x00, 16}, /* Touch probe function */
	    {0x603f, 0x00, 16}, /* Error code */
	    {0x6041, 0x00, 16}, /* Statusword */
	    {0x6061, 0x00, 8}, /* Modes of operation display */
	    {0x6064, 0x00, 32}, /* Position actual value */
	    {0x60b9, 0x00, 16}, /* Touch probe status */
	    {0x60ba, 0x00, 32}, /* Touch probe pos1 pos value */
	    {0x60f4, 0x00, 32}, /* Following error actual value */
	    {0x60fd, 0x00, 32}, /* Digital inputs */
};

ec_pdo_info_t slave_pdos[] = {
    {0x1600, 4, slave_pdo_entries}, /* Receive PDO mapping 1 */
    {0x1a00, 8, slave_pdo_entries + 4}, /* Transmit PDO mapping 1 */
};

ec_sync_info_t slave_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_pdos, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_pdos + 1, EC_WD_DISABLE},
    {0xff} // List end when traversing
};

// Offsets mapped in domain pd
static uint32_t off_control_word_slaves[6] = {0};
static uint32_t off_target_position_slaves[6] = {0};
static uint32_t off_position_actual_value_slaves[6] = {0};
static uint32_t off_status_word_slaves[6] = {0};

#define NUM_SLAVES 1

ec_pdo_entry_reg_t domain1_regs[NUM_SLAVES * 4 + 1];

void configure_pdo_entries()
{
    for (uint16_t i = 0; i < NUM_SLAVES; i++)
    {
        domain1_regs[i * 4 + 0] = (ec_pdo_entry_reg_t){0, i, PANASONIC_A6BE, 0x6040, 0x00, &off_control_word_slaves[i], NULL};
        domain1_regs[i * 4 + 1] = (ec_pdo_entry_reg_t){0, i, PANASONIC_A6BE, 0x6041, 0x00, &off_status_word_slaves[i], NULL};
//        domain1_regs[i * 6 + 2] = (ec_pdo_entry_reg_t){0, i, PANASONIC_A6BE, 0x6060, 0x00, &off_modes_of_operation_slaves[i], NULL};
//        domain1_regs[i * 6 + 3] = (ec_pdo_entry_reg_t){0, i, PANASONIC_A6BE, 0x6061, 0x00, &off_modes_of_operation_display_slaves[i], NULL};
        domain1_regs[i * 4 + 2] = (ec_pdo_entry_reg_t){0, i, PANASONIC_A6BE, 0x6064, 0x00, &off_position_actual_value_slaves[i], NULL};
        domain1_regs[i * 4 + 3] = (ec_pdo_entry_reg_t){0, i, PANASONIC_A6BE, 0x607a, 0x00, &off_target_position_slaves[i], NULL};

    }
    domain1_regs[NUM_SLAVES * 4] = (ec_pdo_entry_reg_t){};
}


void rt_check_domain_state(void)
{
	ec_domain_state_t ds = {};

	ecrt_domain_state(domain1, &ds);

    if (ds.working_counter != domain1_state.working_counter)
    {
		printf("Domain1 : WC %u.\n", ds.working_counter);
	}

    if (ds.wc_state != domain1_state.wc_state)
    {
		printf("Domain1 : State %u.\n", ds.wc_state);
	}

	domain1_state = ds;
}

void rt_check_master_state(void)
{
	ec_master_state_t ms;

	ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding)
    {
		printf("%u slave(s).\n", ms.slaves_responding);
	}

    if (ms.al_states != master_state.al_states)
    {
		printf("AL states: 0x%02X.\n", ms.al_states);
	}

    if (ms.link_up != master_state.link_up)
    {
		printf("Link is %s.\n", ms.link_up ? "up" : "down");
	}

	master_state = ms;
}

static int pds_status[NUM_SLAVES] = {0};
static int pds_status_fsm[NUM_SLAVES] = {0};

void rt_fsm_downward_translate(uint8_t slave_index)
{
    if (pds_status_fsm[slave_index] != pds_status[slave_index])
    {
        if (TEST_MASK(pds_status[slave_index], SW_SWITCH_ON_DISABLED))
        {
            EC_WRITE_U16(domain1_pd + off_control_word_slaves[slave_index], CW_SHUTDOWN);
		}
        if (TEST_MASK(pds_status[slave_index], SW_READY_TO_SWITCH_ON))
        {
            EC_WRITE_U16(domain1_pd + off_control_word_slaves[slave_index], CW_SWITCH_ON);
		}
        if (TEST_MASK(pds_status[slave_index], SW_SWITCHED_ON))
        {
            position_slaves[slave_index] = EC_READ_U32(domain1_pd + off_position_actual_value_slaves[slave_index]);
            EC_WRITE_U32(domain1_pd + off_target_position_slaves[slave_index], position_slaves[slave_index]);
            EC_WRITE_U16(domain1_pd + off_control_word_slaves[slave_index], CW_ENABLE_OPERATION);
		}
	}
	pds_status_fsm[slave_index] = pds_status[slave_index];
}
	
void rt_check_pds_status(uint8_t slave_index) {
	int status_word = EC_READ_U16(domain1_pd + off_status_word_slaves[slave_index]);
	// printf("###start check pds status\n");
	if (status_word != pds_status[slave_index]) {
			printf("Slave %d Status: 0x%x ", slave_index, status_word);
		if (TEST_MASK(status_word, SW_SWITCH_ON_DISABLED)) {
			printf("- Switch on disabled");
		}
		if (TEST_MASK(status_word, SW_READY_TO_SWITCH_ON)) {
			printf(" - Ready to switch on");
		}
		if (TEST_MASK(status_word, SW_SWITCHED_ON)) {
			printf(" - Switched on");
		}
		if (TEST_MASK(status_word, SW_OPERATION_ENABLED)) {
			printf(" - Operation enabled");
		}
		if (TEST_MASK(status_word, SW_QUICK_STOP_ACTIVE)) {
			printf(" - Quick stop active");
		}
		if (TEST_MASK(status_word, SW_FAULT_REACTIVE)) {
			printf(" - Fault reaction active");
		}
		if (TEST_MASK(status_word, SW_FAULT)) {
			printf(" - Fault");
		}
		printf("\n");
		pds_status[slave_index] = status_word;

        // 如果所有从站都准备好，则设置 all_ready 标志
        if(TEST_MASK(status_word, SW_OPERATION_ENABLED)) {
			// printf("###read SW_OPERATION_ENABLED \n");
            ready_count++;
            if (ready_count == NUM_SLAVES) {
                all_ready = 1;
				printf("### slaves all ready \n");
            }
        }
    }
}
// void input_test()
// {
// 	printf("Please Input: ");
// 	scanf("%c", &ch);
// 	if (cmd == 'Q' || cmd == 'q')
// 	{
// 		rt_task_join(&rtask);
// 		rt_task_delete(&rtask);
// 		break;
// 	}
// 	else if(cmd == 'E' || cmd == 'e')
// 	{
// 		for (int i = 0; i < NUM_SLAVES; i++)
// 		{
// 			//enable slave

// 		}
		
// 	}
// 	else if(cmd == 'M' || cmd == 'm')
// 	{
// 		//set slave and move
// 	}
// 	else
// 	{
// 		printf("***************HELP*************\n");
// 		printf("** E | e: Slave Enable        **\n");
// 		printf("** D | d: Slave Disable       **\n");
// 		printf("** M | m: Slave Motion        **\n");
// 		printf("** Q | q: Task Exit           **\n");
// 		printf("********************************\n");
// 	}
// }

#include <fcntl.h>

void rt_task(void* arg) {
	int counter = 0;

	rt_task_set_periodic(NULL, TM_NOW, PERIOD_NS);
	int boot_up = 0;
	uint32_t target_position_slave0;

	unsigned long overrun_sum = 0;
	unsigned long overrun = 0;
#if SYNC_TO_MASTER
	int sync_ref_counter = 0;
#endif

	while (run) {
		overrun = 0;
		rt_task_wait_period(&overrun);
		overrun_sum += overrun;

		counter++;
#if SYNC_TO_MASTER
		ecrt_master_application_time(master, rt_timer_read());
#endif


		ecrt_master_receive(master);
		ecrt_domain_process(domain1);
		//检测domain状态
		rt_check_domain_state();
		// printf("####check domain \n");

		if (!(counter % 1000)) {
			rt_check_master_state();
			// printf("###check master\n");
			if(overrun_sum)
				printf("\nOverruns: %lu\n", overrun_sum);
		}

		for (int i = 0; i < NUM_SLAVES; i++) {
			rt_check_pds_status(i);
			// printf("####check status\n");
			if (!all_ready) {
				rt_fsm_downward_translate(i);//更新状态
			}
		}

		if (all_ready) {
			// printf("####start run write sin\n");
            for (int i = 0; i < NUM_SLAVES; i++) {
                position_slaves[i] = EC_READ_U32(domain1_pd + off_position_actual_value_slaves[i]);
                EC_WRITE_U32(domain1_pd + off_target_position_slaves[i],
                                position_slaves[i] + sin((counter % 20000) * M_PI / 1000) * 10000);
            }
		}


#if SYNC_TO_MASTER
	//十个周期同步时钟
        // 周期性同步主站参考时钟
		if (sync_ref_counter == 0) {
			sync_ref_counter = 10; 
			ecrt_master_sync_reference_clock(master);
		} else {
			sync_ref_counter--;
		}

		ecrt_master_sync_slave_clocks(master);//从站时钟同同步
#endif

		rt_task_wait_period(NULL);
		ecrt_master_receive(master);
		ecrt_domain_process(domain1);
		// for (int i = 0; i < NUM_SLAVES; i++) {
		// 	EC_WRITE_U16(domain1_pd + off_control_word_slaves[i], CW_DISABLE_OPERATION);
		// 	rt_check_pds_status(i);
		// }

		ecrt_domain_queue(domain1);
		ecrt_master_send(master);
	// }

	//END
	//PDS Disable Operation
	//
	// for(int i = 0; i< 20000; i++)
	// {
		//等待周期任务结束
		// rt_task_wait_period(NULL);

		// ecrt_master_receive(master);//接收主站数据
		// ecrt_domain_process(domain1);//调用线程进行domain数据处理
		// //从站disable
		// for (int i = 0; i < NUM_SLAVES; i++) {
		// 	EC_WRITE_U16(domain1_pd + off_control_word_slaves[i], CW_DISABLE_OPERATION);
		// 	rt_check_pds_status(i);
		// }
		// //数据进入domain的队列
		// ecrt_domain_queue(domain1);
		// //通过网卡发送数据
		// ecrt_master_send(master);

/*
	printf("Please Input: ");
	scanf("%c", &ch);
	if (cmd == 'Q' || cmd == 'q')
	{
		rt_task_join(&rtask);
		rt_task_delete(&rtask);
	}
	else if(cmd == 'E' || cmd == 'e')
	{
		for (int i = 0; i < NUM_SLAVES; i++)
		{
			//enable slave

		}
		
	}
	else if(cmd == 'M' || cmd == 'm')
	
*/	
	}

	 
}

void signal_handler(int sig)
{
	run = 0;
}

int main() {

	signal(SIGTERM, signal_handler);
	signal(SIGINT, signal_handler);

	mlockall(MCL_CURRENT | MCL_FUTURE);

	master = ecrt_request_master(0);
	if (!master) {
		return -1;
	}

	uint32_t data = 0;
	uint32_t abort_code = 0;
	size_t result_size = 0;
	int ret;
#if 1
	ret = ecrt_master_sdo_upload(master, 0, INDEX_VENDOR_ID, reinterpret_cast<uint8_t*>(&data), sizeof(data), &result_size, &abort_code);
	if(!ret)
		printf("Vendor ID: %x\r\n", data);
	ecrt_master_sdo_upload(master, 0, INDEX_PRODUCT_CODE, reinterpret_cast<uint8_t*>(&data), sizeof(data), &result_size, &abort_code);
	if(!ret)
		printf("Product Code: %x\r\n", data);

	int8_t mode_of_operation = 0x8;
	ret = ecrt_master_sdo_download(master, 0, 0x6060, 0x0, reinterpret_cast<uint8_t*>(&mode_of_operation), sizeof(mode_of_operation), &abort_code);
	if(ret)
		printf("Download Error!\n");
#endif
	domain1 = ecrt_master_create_domain(master);
	if (!domain1) {
		printf("create domain Error!\n");
	}

	configure_pdo_entries();
    printf("Populate the pdo map \n");

    for (int i = 0; i < NUM_SLAVES; i++) {
        slaves[i] = ecrt_master_slave_config(master, 0, i, PANASONIC_A6BE);
        if (!slaves[i]) {
            printf("Failed to configure slave %d\n", i);
            return -1;
        }
		if (ecrt_slave_config_pdos(slaves[i], EC_END, slave_syncs)) {
        	printf("Failed to configure PDOs for slave %d\n", i);
            return -1;
        }

		ecrt_slave_config_dc(slaves[i], 0x0330, PERIOD_NS, 0, 0, 0);

	}

    printf("config dc \n");
    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
        printf("Failed to register PDO entries\n");
        return -1;
    }else{
    	printf("write domain1_regs \n");
    }
	printf("Activating master... cyclic time: %d\n", PERIOD_NS);
	
	// //configure dc before master start
	// ecrt_slave_config_dc(slave0, ACTIVATE_CYCLIC_OPERATION |
	// 		ACTIVATE_SYNC0,
	// 		PERIOD_NS, 0, 0, 0);

	printf("Activating master...\n");
	if (ecrt_master_activate(master)) {
		return -1;
	}

	if (!(domain1_pd = ecrt_domain_data(domain1))) {
	}
	sleep(1);
	ret = rt_task_create(&my_task, "my_task", 0, 75, T_FPU | T_JOINABLE);
	if (ret < 0) {
		return -1;
	}

	ret = rt_task_start(&my_task, &rt_task, NULL);
	if (ret < 0) {
        printf("Failed to create task\n");
		return -1;
	}	

	while (run) {
		sched_yield();
	}

	rt_task_join(&my_task);

	rt_task_delete(&my_task);

	ecrt_master_deactivate(master);
	ecrt_release_master(master);
	printf("\n");
	return 0;
}
